/**
 * Part of the Rigid Body Physics Simulator
 * Adapted for the New Version of Pong
 * Created by Wilson Burhan and Tyler Simrell
 * Using a code base by Matthew Alden
 * Date Finished: 5/24/13
 * Date Modified: 6/12/13
 * Version: 1.5
 */

//CollisionHandler.java
//Code base by Matthew Alden
//Modified by Tyler Simrell and Wilson Burhan

import javax.vecmath.*;

public class CollisionHandler {
	private static final float COEFFICIENT_OF_RESTITUTION = 1.05f;
	private static final float COEFFICIENT_OF_FRICTION = .1f;
	
	public static void checkAndResolveCollision(PhysicsObject a, PhysicsObject b) {
		CollisionInfo ci = getCollisionInfo(a, b);
		if (ci == null)
			return;

		// Vector from the center of mass of object a to the collision point
		Vector2f r_ap = new Vector2f();
		r_ap.scaleAdd(-1, a.getGlobalCenterOfMass(), ci.position);
		// Vector from the center of mass of object b to the collision point
		Vector2f r_bp = new Vector2f();
		r_bp.scaleAdd(-1, b.getGlobalCenterOfMass(), ci.position);
		// Velocity of object a at the point of collision
		Vector2f v_ap1 = new Vector2f();
		v_ap1.x = a.velocity.x - a.angularVelocity * r_ap.y;
		v_ap1.y = a.velocity.y + a.angularVelocity * r_ap.x;
		// Velocity of object b at the point of collision
		Vector2f v_bp1 = new Vector2f();
		v_bp1.x = b.velocity.x - b.angularVelocity * r_bp.y;
		v_bp1.y = b.velocity.y + b.angularVelocity * r_bp.x;
		// The collision impulse
		Vector2f v_ab1 = new Vector2f();
		v_ab1.scaleAdd(-1, v_bp1, v_ap1);
		float tmpA = r_ap.x * ci.normal.y - r_ap.y * ci.normal.x;
		float tmpB = r_bp.x * ci.normal.y - r_bp.y * ci.normal.x;		
		float j = -(1 + COEFFICIENT_OF_RESTITUTION) * (v_ab1.dot(ci.normal)) / (1 / a.mass + 1 / b.mass + tmpA * tmpA / a.momentOfInertia + tmpB * tmpB / b.momentOfInertia);
		
		Vector2f tangent = new Vector2f();
		
		Vector2f diffVelocity = new Vector2f();
		diffVelocity.sub(b.velocity, a.velocity);
		
		// Update object a's velocity
		a.velocity.scaleAdd(j / a.mass, ci.normal, a.velocity);
		
		tangent.sub(diffVelocity, ci.normal);
		tangent.dot(ci.normal);
		tangent.normalize();
		
		float impulse = a.velocity.dot(tangent) * COEFFICIENT_OF_FRICTION;
		tangent.scale(impulse);
		a.velocity.sub(tangent);
		
		// Update object b's velocity
		b.velocity.scaleAdd(-j / b.mass, ci.normal, b.velocity);
		
		tangent = new Vector2f();
		tangent.sub(diffVelocity, ci.normal);
		tangent.dot(ci.normal);
		tangent.normalize();
		
		impulse = b.velocity.dot(tangent) * COEFFICIENT_OF_FRICTION;
		tangent.scale(impulse);
		b.velocity.sub(tangent);
		
		// Update object a's angular velocity
		a.angularVelocity += j * (r_ap.x * ci.normal.y - r_ap.y * ci.normal.x) / a.momentOfInertia;
		// Update object b's angular velocity
		b.angularVelocity -= j * (r_bp.x * ci.normal.y - r_bp.y * ci.normal.x) / b.momentOfInertia;
		// Remove object overlap
		a.position.scaleAdd(-ci.depth / (a.mass * (1 / a.mass + 1 / b.mass)), ci.normal, a.position);
		b.position.scaleAdd(ci.depth / (b.mass * (1 / a.mass + 1 / b.mass)), ci.normal, b.position);
		
		a.clearCaches();
		b.clearCaches();
	}
	
	private static CollisionInfo getCollisionInfo(PhysicsObject a, PhysicsObject b) {
		if (a == b)
			return null;
		
		CollisionInfo ci = null;
		if (a instanceof HalfSpace) 
		{
			if (b instanceof Circle)
				ci = getCollision((HalfSpace)a, (Circle)b);
			else if (b instanceof Paddle)
				ci = getCollision((HalfSpace)a, (Paddle)b);
			else if (b instanceof Triangle)
			{
				ci = getCollision((HalfSpace) a, (Triangle) b);
			}
		}
		
		else if (a instanceof Circle) 
		{
			if (b instanceof Circle)
				ci = getCollision((Circle)a, (Circle)b);
			else if(b instanceof Paddle) 
				ci = getCollision((Paddle) b, (Circle)a);
		}
		else if (a instanceof Paddle)
		{
			if(b instanceof Circle)
			{
				ci = getCollision((Paddle) a, (Circle) b);
			}
			
			else if(b instanceof Triangle)
			{
				ci = getCollision((Paddle) a , (Triangle) b);
			}
		}
		return ci;
	}

	
	
	/*
	private static CollisionInfo getCollision(Triangle a, Square b) {
		Vector2f[] verticesTri = a.getVertices();
		Vector2f[] normalsTriA = a.getNormals();
		Vector2f[] verticesSquare = b.getVertices();
		Vector2f[] normalsSquareB = b.getNormals();
		float[][] distanceFromA = new float[verticesTri.length][verticesSquare.length];
		float[][] distanceFromB = new float[verticesSquare.length][verticesTri.length];
		int[] indexMinDistanceFromA = new int[verticesTri.length];
		int[] indexMinDistanceFromB = new int[verticesSquare.length];
		
		for (int i = 0; i < verticesTri.length; i++) {
			for (int j = 0; j < verticesSquare.length; j++) {
				Vector2f tmp = new Vector2f();
				tmp.scaleAdd(-1, verticesTri[i], verticesSquare[j]);
				distanceFromA[i][j] = tmp.dot(normalsTriA[i]);
				if (distanceFromA[i][j] < distanceFromA[i][indexMinDistanceFromA[i]])
					indexMinDistanceFromA[i] = j;
			}
			if (distanceFromA[i][indexMinDistanceFromA[i]] >= 0)
				return null;
		}
		for (int i = 0; i < verticesSquare.length; i++) {
			for (int j = 0; j < verticesTri.length; j++) {
				Vector2f tmp = new Vector2f(verticesTri[j]);
				tmp.scaleAdd(-1, verticesSquare[i], verticesTri[j]);
				distanceFromB[i][j] = tmp.dot(normalsSquareB[i]);
				if (distanceFromB[i][j] < distanceFromB[i][indexMinDistanceFromB[i]])
					indexMinDistanceFromB[i] = j;
			}
			if (distanceFromB[i][indexMinDistanceFromB[i]] >= 0)
				return null;
		}
		
		int indexMaxDistanceFromA = 0;
		for (int i = 1; i < verticesTri.length; i++)
			if (distanceFromA[i][indexMinDistanceFromA[i]] > distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]])
				indexMaxDistanceFromA = i;
		int indexMaxDistanceFromB = 0;
		for (int i = 1; i < verticesSquare.length; i++)
			if (distanceFromB[i][indexMinDistanceFromB[i]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]])
				indexMaxDistanceFromB = i;
		
		CollisionInfo ci = new CollisionInfo();
		if (distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]]) {
			ci.depth = -distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]];
			ci.normal = new Vector2f(normalsTriA[indexMaxDistanceFromA]);
			ci.position = new Vector2f(verticesSquare[indexMinDistanceFromA[indexMaxDistanceFromA]]);
			ci.position.scaleAdd(-ci.depth, ci.normal, ci.position);
		} else {
			ci.depth = -distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]];
			ci.normal = new Vector2f(normalsSquareB[indexMaxDistanceFromB]);
			ci.normal.scale(-1);
			ci.position = new Vector2f(verticesTri[indexMinDistanceFromB[indexMaxDistanceFromB]]);
	//		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		}
		return ci;
	}

	private static CollisionInfo getCollision(Square a, Square b) {
		Vector2f[] verticesSquare1 = a.getVertices();
		Vector2f[] normalsA = a.getNormals();
		Vector2f[] verticesSquare2 = b.getVertices();
		Vector2f[] normalsB = b.getNormals();
		float[][] distanceFromA = new float[verticesSquare1.length][verticesSquare2.length];
		float[][] distanceFromB = new float[verticesSquare2.length][verticesSquare1.length];
		int[] indexMinDistanceFromA = new int[verticesSquare1.length];
		int[] indexMinDistanceFromB = new int[verticesSquare2.length];
		
		for (int i = 0; i < verticesSquare1.length; i++) {
			for (int j = 0; j < verticesSquare2.length; j++) {
				Vector2f tmp = new Vector2f();
				tmp.scaleAdd(-1, verticesSquare1[i], verticesSquare2[j]);
				distanceFromA[i][j] = tmp.dot(normalsA[i]);
				if (distanceFromA[i][j] < distanceFromA[i][indexMinDistanceFromA[i]])
					indexMinDistanceFromA[i] = j;
			}
			if (distanceFromA[i][indexMinDistanceFromA[i]] >= 0)
				return null;
		}
		for (int i = 0; i < verticesSquare2.length; i++) {
			for (int j = 0; j < verticesSquare1.length; j++) {
				Vector2f tmp = new Vector2f(verticesSquare1[j]);
				tmp.scaleAdd(-1, verticesSquare2[i], verticesSquare1[j]);
				distanceFromB[i][j] = tmp.dot(normalsB[i]);
				if (distanceFromB[i][j] < distanceFromB[i][indexMinDistanceFromB[i]])
					indexMinDistanceFromB[i] = j;
			}
			if (distanceFromB[i][indexMinDistanceFromB[i]] >= 0)
				return null;
		}
		
		int indexMaxDistanceFromA = 0;
		for (int i = 1; i < verticesSquare1.length; i++)
			if (distanceFromA[i][indexMinDistanceFromA[i]] > distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]])
				indexMaxDistanceFromA = i;
		int indexMaxDistanceFromB = 0;
		for (int i = 1; i < verticesSquare2.length; i++)
			if (distanceFromB[i][indexMinDistanceFromB[i]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]])
				indexMaxDistanceFromB = i;
		
		CollisionInfo ci = new CollisionInfo();
		if (distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]]) {
			ci.depth = -distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]];
			ci.normal = new Vector2f(normalsA[indexMaxDistanceFromA]);
			ci.position = new Vector2f(verticesSquare2[indexMinDistanceFromA[indexMaxDistanceFromA]]);
			ci.position.scaleAdd(-ci.depth, ci.normal, ci.position);
		} else {
			ci.depth = -distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]];
			ci.normal = new Vector2f(normalsB[indexMaxDistanceFromB]);
			ci.normal.scale(-1);
			ci.position = new Vector2f(verticesSquare1[indexMinDistanceFromB[indexMaxDistanceFromB]]);
	//		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		}
		return ci;
	}
*/
	private static CollisionInfo getCollision(HalfSpace a, Circle b) 
	{
		float distance = a.normal.dot(b.position) - a.intercept - b.radius;
		if (distance < 0) {
			CollisionInfo ci = new CollisionInfo();
			ci.normal = a.normal;
			ci.depth = -distance;
			ci.position = new Vector2f();
			ci.position.scaleAdd(-(b.radius - ci.depth), ci.normal, b.position);
			return ci;
		}
		return null;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Triangle b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];
		
		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		
		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;
		
		CollisionInfo ci = new CollisionInfo();
		ci.depth = -distances[minIndex];
		ci.normal = a.normal;
		ci.position = new Vector2f(vertices[minIndex]);
		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		return ci;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Paddle b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];
		
		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		
		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;
		
		CollisionInfo ci = new CollisionInfo();
		ci.depth = -distances[minIndex];
		ci.normal = a.normal;
		ci.position = new Vector2f(vertices[minIndex]);
		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		return ci;
	}
	
	private static CollisionInfo getCollision(Circle a, Circle b) {
		Vector2f n = new Vector2f();
		n.scaleAdd(-1, a.position, b.position);
		float distance = n.length() - a.radius - b.radius;
		if (distance < 0) {
			CollisionInfo ci = new CollisionInfo();
			n.normalize();
			ci.normal = n;
			ci.depth = -distance;
			ci.position = new Vector2f();
			ci.position.scaleAdd(a.radius - ci.depth / 2, ci.normal, a.position);
			return ci;
		}
		return null;
	}
	/*
	private static CollisionInfo getCollision(Triangle a, Triangle b) {
		Vector2f[] verticesTri1 = a.getVertices();
		Vector2f[] normalsA = a.getNormals();
		Vector2f[] verticesTri2 = b.getVertices();
		Vector2f[] normalsB = b.getNormals();
		float[][] distanceFromA = new float[verticesTri1.length][verticesTri2.length];
		float[][] distanceFromB = new float[verticesTri2.length][verticesTri1.length];
		int[] indexMinDistanceFromA = new int[verticesTri1.length];
		int[] indexMinDistanceFromB = new int[verticesTri2.length];
		
		for (int i = 0; i < verticesTri1.length; i++) {
			for (int j = 0; j < verticesTri2.length; j++) {
				Vector2f tmp = new Vector2f();
				tmp.scaleAdd(-1, verticesTri1[i], verticesTri2[j]);
				distanceFromA[i][j] = tmp.dot(normalsA[i]);
				if (distanceFromA[i][j] < distanceFromA[i][indexMinDistanceFromA[i]])
					indexMinDistanceFromA[i] = j;
			}
			if (distanceFromA[i][indexMinDistanceFromA[i]] >= 0)
				return null;
		}
		for (int i = 0; i < verticesTri2.length; i++) {
			for (int j = 0; j < verticesTri1.length; j++) {
				Vector2f tmp = new Vector2f(verticesTri1[j]);
				tmp.scaleAdd(-1, verticesTri2[i], verticesTri1[j]);
				distanceFromB[i][j] = tmp.dot(normalsB[i]);
				if (distanceFromB[i][j] < distanceFromB[i][indexMinDistanceFromB[i]])
					indexMinDistanceFromB[i] = j;
			}
			if (distanceFromB[i][indexMinDistanceFromB[i]] >= 0)
				return null;
		}
		
		int indexMaxDistanceFromA = 0;
		for (int i = 1; i < verticesTri1.length; i++)
			if (distanceFromA[i][indexMinDistanceFromA[i]] > distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]])
				indexMaxDistanceFromA = i;
		int indexMaxDistanceFromB = 0;
		for (int i = 1; i < verticesTri2.length; i++)
			if (distanceFromB[i][indexMinDistanceFromB[i]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]])
				indexMaxDistanceFromB = i;
		
		CollisionInfo ci = new CollisionInfo();
		if (distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]]) {
			ci.depth = -distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]];
			ci.normal = new Vector2f(normalsA[indexMaxDistanceFromA]);
			ci.position = new Vector2f(verticesTri2[indexMinDistanceFromA[indexMaxDistanceFromA]]);
			ci.position.scaleAdd(-ci.depth, ci.normal, ci.position);
		} else {
			ci.depth = -distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]];
			ci.normal = new Vector2f(normalsB[indexMaxDistanceFromB]);
			ci.normal.scale(-1);
			ci.position = new Vector2f(verticesTri1[indexMinDistanceFromB[indexMaxDistanceFromB]]);
	//		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		}
		return ci;
	}
	
	private static CollisionInfo getCollision(Triangle tri, Circle circle)
	{
		Vector2f[] verticesTri = tri.getVertices();
		Vector2f[] normalsTri = tri.getNormals();
		
		float[] distanceFromTri = new float[verticesTri.length];
		for (int i = 0; i < verticesTri.length; i++) 
		{
			Vector2f tmp = new Vector2f();
			tmp.scaleAdd(-1, verticesTri[i], circle.position);
			distanceFromTri[i] = tmp.dot(normalsTri[i]);
			distanceFromTri[i] -= circle.radius;
			if (distanceFromTri[i] >= 0)
				return null;
		}
		
		int indexMaxDistanceFromTri = 0;
		for (int i = 1; i < verticesTri.length; i++)
		{
			if (distanceFromTri[i] > distanceFromTri[indexMaxDistanceFromTri])
			{
				indexMaxDistanceFromTri = i;
			}
		}
		
		CollisionInfo ci = new CollisionInfo();
		
		ci.depth = -distanceFromTri[indexMaxDistanceFromTri];
		ci.normal = new Vector2f(normalsTri[indexMaxDistanceFromTri]);
		ci.position = new Vector2f();
		ci.position.scaleAdd(circle.radius, ci.normal, circle.position);
		
		return ci;
	}
	*/
	private static CollisionInfo getCollision(Paddle paddle, Circle circle)
	{
		if(paddle.position.y < circle.position.y && 
				circle.position.y < (paddle.position.y + 2.5f))
		{
			float distance = paddle.normal.dot(circle.position) - 
					(paddle.intercept + .25f) - circle.radius;
			
			if (distance < 0) {
				CollisionInfo ci = new CollisionInfo();
				ci.normal = paddle.normal;
				ci.depth = -distance;
				ci.position = new Vector2f();
				ci.position.scaleAdd(-(circle.radius - ci.depth), ci.normal, circle.position);
				return ci;
			}
		}
		return null;
	}
	
	private static CollisionInfo getCollision(Paddle paddle, Triangle tri)
	{
		if(paddle.position.y < tri.position.y && 
				tri.position.y < (paddle.position.y + 2.5f))
		{
			Vector2f[] vertices = tri.getVertices();
			float[] distances = new float[vertices.length];
			
			for (int i = 0; i < vertices.length; i++)
				distances[i] = paddle.normal.dot(vertices[i]) - 
				(paddle.intercept + .25f);
			
			int minIndex = 0;
			for (int i = 1; i < distances.length; i++)
				if (distances[i] < distances[minIndex])
					minIndex = i;
			if (distances[minIndex] >= 0)
				return null;
			
			CollisionInfo ci = new CollisionInfo();
			ci.depth = -distances[minIndex];
			ci.normal = paddle.normal;
			ci.position = new Vector2f(vertices[minIndex]);
			ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
			return ci;
		}
		
		return null;
	}
}
